//
// Created by fl on 23-9-6.
//

#include "px4_interface.h"


Px4Interface::Px4Interface(int recv_port, int send_port, const std::string& px4_ip) {
    if (!initUDP(recv_port, send_port, px4_ip)) {
        perror("init PX4 telem failed!!!");
        exit(0);
    }
    
    std::thread thread_recv_px4(&Px4Interface::recvPX4byUDP, this);
    thread_recv_px4.detach();

    std::thread thread_decode_mavmsg(&Px4Interface::decodeUDPmsg, this);
    thread_decode_mavmsg.detach();

    std::thread thread_heart(&Px4Interface::sendHeartbeat, this);
    thread_heart.detach();
}

Px4Interface::~Px4Interface() {
    terminate_offboard();
    shutdown(sock_fd_, SHUT_RDWR);
    close(sock_fd_);
};

UavStatus Px4Interface::getPx4Status() {
    UavStatus data;
    std::unique_lock<std::mutex> lock(px4_info_mtx_);
    data = px4_info_;
    lock.unlock();
    return data;
}

bool Px4Interface::updated(UavStatus &px4_info) {
    if (updated_) {
        updated_ = false;
        px4_info = getPx4Status();
        return true;
    } else {
        return false;
    }
}

void Px4Interface::send_att_cmd_to_px4(mavlink_set_attitude_target_t &cmd) const {
    mavlink_message_t  msg;

    cmd.target_system = sys_id_;
    cmd.target_component = MAV_COMP_ID_AUTOPILOT1;
//    cmd.q
    mavlink_msg_set_attitude_target_encode(222, 255, &msg, &cmd);
    send_msg_to_px4(msg);
    set_offboard_mode();
}

void Px4Interface::send_euler_cmd_to_px4(float roll, float pitch, float yaw, 
                            float throttle)
{
    mavlink_set_attitude_target_t cmd;
    matrix::Eulerf euler(roll, pitch, yaw);
    matrix::Quatf cmd_q = euler;

    cmd.time_boot_ms = currentTimeMs();
    cmd.q[0] = cmd_q(0);
    cmd.q[1] = cmd_q(1);
    cmd.q[2] = cmd_q(2);
    cmd.q[3] = cmd_q(3);
    cmd.thrust = throttle;

    cmd.type_mask = 0b00000111;
    cmd.target_system = sys_id_;
    cmd.target_component = MAV_COMP_ID_AUTOPILOT1;
    send_att_cmd_to_px4(cmd);
}

void Px4Interface::send_local_pos_target_to_px4(float n, float e, float d)
{
    mavlink_set_position_target_local_ned_t local_setpoint;
    local_setpoint.x = n;
    local_setpoint.y = e;
    local_setpoint.z = d;
    local_setpoint.target_component = MAV_COMP_ID_AUTOPILOT1;
    local_setpoint.target_system = sys_id_;
    local_setpoint.type_mask = 0b0000111111111000;
    local_setpoint.coordinate_frame = MAV_FRAME_LOCAL_NED;
    mavlink_message_t msg;
    mavlink_msg_set_position_target_local_ned_encode(222, 250, 
    &msg, &local_setpoint);
    send_msg_to_px4(msg);
    set_offboard_mode();
}

void Px4Interface::set_offboard_mode() const {
    if (main_mode_ != PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
        mavlink_command_int_t cmd{};
        mavlink_message_t msg{};
        memset(&cmd, 0, sizeof(cmd));
        cmd.target_system = sys_id_; // 和QGC的飞控id对应
        cmd.target_component = MAV_COMP_ID_ALL;
        cmd.command = MAV_CMD::MAV_CMD_DO_SET_MODE;
        cmd.param1 = 1;
        cmd.param2 = 6; // 6 -> offboard
        mavlink_msg_command_int_encode(222, 255, &msg, &cmd);
        send_msg_to_px4(msg);
        std::cout << "set mode to offboard " << std::endl;
    }
}

void Px4Interface::set_arm() const {
    if (!(my_heartbeat_.base_mode & MAV_MODE_FLAG_SAFETY_ARMED)) {
        mavlink_command_int_t cmd{};
        mavlink_message_t msg{};
        memset(&cmd, 0, sizeof(cmd));
        cmd.target_system = sys_id_;
        cmd.target_component = MAV_COMP_ID_ALL;
        cmd.command = MAV_CMD::MAV_CMD_COMPONENT_ARM_DISARM;
        cmd.param1 = 1;
        cmd.param2 = 21196;
        mavlink_msg_command_int_encode(222, 255, &msg, &cmd);
        send_msg_to_px4(msg);
        std::cout << "set arm" << std::endl;
    }
}

void Px4Interface::set_mission_mode() const {
    mavlink_command_int_t cmd{};
    mavlink_message_t msg{};
    memset(&cmd, 0, sizeof(cmd));
    cmd.target_system = sys_id_; // 和QGC的飞控id对应
    cmd.target_component = MAV_COMP_ID_ALL;
    cmd.command = MAV_CMD::MAV_CMD_DO_SET_MODE;
    cmd.param1 = 1;
    cmd.param2 = 4; // 6 -> offboard
    mavlink_msg_command_int_encode(222, 255, &msg, &cmd);
    send_msg_to_px4(msg);
    std::cout << "set mode to mission " << std::endl;
}

bool Px4Interface::initUDP(int recv_port, int send_port, const std::string& px4_ip) {

    st_recv_addr_.sin_family = AF_INET;
    st_recv_addr_.sin_addr.s_addr = htonl(INADDR_ANY);
    st_recv_addr_.sin_port = htons(recv_port);
    sock_fd_ = socket(AF_INET, SOCK_DGRAM, 0);
    if (::bind(sock_fd_, (struct sockaddr *)&st_recv_addr_, sizeof(struct sockaddr)) == -1) {
        std::cout << "port " << recv_port << std::endl;
        perror(" st_recv_addr_ bind failed!!!");
        return false;
    }

    st_send_addr_.sin_family = AF_INET;
    st_send_addr_.sin_addr.s_addr = inet_addr(px4_ip.c_str());
    st_send_addr_.sin_port = htons(send_port);

    return true;
}

void Px4Interface::recvPX4byUDP() {
    socklen_t fromlen = sizeof(struct sockaddr);
    std::unique_lock<std::mutex> lock(udpdata_mtx_, std::defer_lock);
    while (true)
    {
        char buffer[1024];
        ssize_t recv_len = recvfrom(sock_fd_, &buffer, 1024, 0,
                                    (struct sockaddr *)&st_recv_addr_, &fromlen);
        if (recv_len > 0)
        {
            lock.lock();
            for (int i = 0; i < recv_len; i++)
            {
                udpdata_.push(buffer[i]);
            }
            lock.unlock();
        } else {
            usleep(1);
        }
    }
}

void Px4Interface::decodeUDPmsg() {
    mavlink_message_t msg;
    mavlink_status_t status;

    mavlink_global_position_int_t globalpos;
    mavlink_attitude_quaternion_t att_q;
    mavlink_home_position_t home_pos;
    mavlink_vfr_hud_t vfrHud;
    mavlink_local_position_ned_t local_pos;

    std::unique_lock<std::mutex> lock_1(udpdata_mtx_, std::defer_lock);
    std::unique_lock<std::mutex> lock_2(px4_info_mtx_, std::defer_lock);

    while (true) {
        if (!udpdata_.empty()) {
            lock_1.lock();
            char byte = udpdata_.front();
            udpdata_.pop();
            lock_1.unlock();
            if (mavlink_parse_char(MAVLINK_COMM_0, byte, &msg, &status)) {
                if (msg.sysid != 255) {
                    if (sys_id_ != msg.sysid) sys_id_ = msg.sysid;
                    switch (msg.msgid) {
                        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
                            mavlink_msg_global_position_int_decode(&msg, &globalpos);
                            lock_2.lock();
                            px4_info_.timestamp = currentTimeMs();
                            px4_info_.globalPos = globalpos;
                            lock_2.unlock();
                            updated_ = true;
                            break;
                        }
                        case MAVLINK_MSG_ID_LOCAL_POSITION_NED: {
                            mavlink_msg_local_position_ned_decode(&msg, &local_pos);
                            lock_2.lock();
                            px4_info_.localPos = local_pos;
                            lock_2.unlock();
                            updated_ = true;
                            break;
                        }
                        case MAVLINK_MSG_ID_ATTITUDE_QUATERNION: {
                            mavlink_msg_attitude_quaternion_decode(&msg, &att_q);
                            lock_2.lock();
                            px4_info_.att_q = att_q;
                            lock_2.unlock();
                            break;
                        }
                        case MAVLINK_MSG_ID_HOME_POSITION: {
                            mavlink_msg_home_position_decode(&msg, &home_pos);
                            lock_2.lock();
                            px4_info_.home_pos = home_pos;
                            lock_2.unlock();
                            break;
                        }
                        case MAVLINK_MSG_ID_HEARTBEAT: {
                            mavlink_msg_heartbeat_decode(&msg, &my_heartbeat_);
                            uint8_t main_mode_tmp = (my_heartbeat_.custom_mode >> 16) & 0xff;
                            if (main_mode_tmp < 10) {
                                main_mode_ = main_mode_tmp;
                            } else {
                                std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;
                            }
                            break;
                        }
                        case MAVLINK_MSG_ID_VFR_HUD: {
                            mavlink_msg_vfr_hud_decode(&msg, &vfrHud);
                            lock_2.lock();
                            px4_info_.vfr_hud = vfrHud;
                            lock_2.unlock();
                            break;
                        }
                        default:
                            break;
                    }
                }
            }
        } else {
            usleep(1);
        }
    }
}

void Px4Interface::sendHeartbeat() {
    mavlink_heartbeat_t heart{.custom_mode = 1, .type = 1, .autopilot = 12,
            .base_mode = 97, .system_status= 8 , .mavlink_version = 3};
    mavlink_message_t msg;
    mavlink_msg_heartbeat_encode(222, 250, &msg, &heart);

    while (true) {
        send_msg_to_px4(msg);
        sleep(1);
    }
}

bool Px4Interface::send_msg_to_px4(const mavlink_message_t &msg) const {
    uint8_t sendbuf[100];
    ssize_t buflen = mavlink_msg_to_send_buffer(sendbuf, &msg);

    ssize_t send_len = sendto(sock_fd_, sendbuf, buflen, 0,
                              (struct sockaddr *) &st_send_addr_, sizeof(struct sockaddr_in));

    if (send_len <= 0) {
        perror("Failed to send msg to px4 by UDP");
        return false;
    } else {
        return true;
    }
}

void Px4Interface::terminate_offboard() const {
    if (main_mode_ == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
        set_mission_mode();
    }
}

void Px4Interface::send_local_target(mavlink_set_position_target_local_ned_t &pos_cmd) {
    pos_cmd.target_system = sys_id_;
    mavlink_message_t msg;
    mavlink_msg_set_position_target_local_ned_encode(222, 250,
                                                     &msg, &pos_cmd);
    send_msg_to_px4(msg);
    set_offboard_mode();
    set_arm();
}
